#include "cam_info.h"
#include "json.hpp"
#include <fstream>

using namespace perception::camera;

CameraInfo *CameraInfo::cam_info_ = NULL;

CameraInfo *CameraInfo::get_inst(){
    if (!cam_info_){
        cam_info_ = new CameraInfo("/home/ubuntu/workroot/etc/inex/");
    }
    return  cam_info_;
}

CameraInfo::CameraInfo(std::string inex_dir, std::string roi_dir)
    : inex_dir_(inex_dir)
    , roi_dir_(roi_dir)
    , logger_(rclcpp::get_logger("CameraInfo"))
{
    std::cout << "in ex dir:" << inex_dir << std::endl;
    for(int i = 0; i < 7; ++i){
        std::string cam_name = "camera" + std::to_string(i);
        std::string inex_path = inex_dir + "/" + cam_name + ".json";
        std::cout << "inexpath:" << inex_path << std::endl;
        std::ifstream inexfile(inex_path);
        assert(inexfile.good());
        nlohmann::json j = nlohmann::json::parse(inexfile);
        cam_K_[cam_name] = cv::Matx<double, 3, 3>(
            j["K"][0][0], j["K"][0][1], j["K"][0][2],
            j["K"][1][0], j["K"][1][1], j["K"][1][2],
            j["K"][2][0], j["K"][2][1], j["K"][2][2]);
        cam_distort_[cam_name] = cv::Matx<double, 1, 5>(j["distort"][0], j["distort"][1], j["distort"][2], j["distort"][3], j["distort"][4]);
        std::cout << cam_name <<  " dist_coeffs_: " << cam_distort_[cam_name] << std::endl;
        cam_c2g_[cam_name] = cv::Matx<double, 3, 3>(
            j["cam2ground"][0][0], j["cam2ground"][0][1], j["cam2ground"][0][2],
            j["cam2ground"][1][0], j["cam2ground"][1][1], j["cam2ground"][1][2],
            j["cam2ground"][2][0], j["cam2ground"][2][1], j["cam2ground"][2][2]);
        std::cout << cam_name << " cam2ground_:  " << cam_c2g_[cam_name] << std::endl;
    }
}

void CameraInfo::convert_uv2xy(const std::string &cam_name, const std::vector<cv::Point2f> &im_pts, std::vector<cv::Point2f> &ground_pts){
    std::vector<cv::Point2f> undistorted;
    std::vector<cv::Point2f> wxy;
    std::vector<cv::Point2f> pts_calib;
    float calibw = cam_calib_wh_.at(cam_name).width;
    float calibh = cam_calib_wh_.at(cam_name).height;
    for(size_t i = 0; i < im_pts.size(); ++i){
        float x = im_pts.at(i).x * calibw;
        float y = im_pts.at(i).y * calibh;
        pts_calib.push_back(cv::Point2f(x, y));
    }
    cv::undistortPoints(pts_calib, undistorted, cam_K_.at(cam_name), cam_distort_.at(cam_name));
    cv::perspectiveTransform(undistorted, ground_pts, cam_c2g_.at(cam_name));
}
